#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_msgs/GridMap.h>
#include <cmath>  // 引入cmath库以使用 std::isinf 和数学函数 [[10]]
#include <vector> // 引入vector用于存储有效点 [[6]]

class ElevationClipper
{
public:
  ElevationClipper(ros::NodeHandle &nh)
  {
    nh.param<std::string>("elevation_input_topic", elevation_input_topic_, "/elevation_map");
    nh.param<std::string>("elevation_output_topic", elevation_output_topic_, "/elevation_map_clip");
    nh.param<double>("resolution", resolution_, 0.2);
    nh.param<double>("map_x_scale", map_x_scale_, 12.0);
    nh.param<double>("map_y_scale", map_y_scale_, 12.0);
    nh.param<double>("origin_x", origin_x_, 1.0);

    // IDW参数（可配置）
    idw_power_ = 2.0;  // 幂参数（通常为2）[[6]]
    idw_radius_ = 2.0; // 搜索半径（米）

    sub_ = nh.subscribe(elevation_input_topic_, 1, &ElevationClipper::mapCallback, this);
    pub_ = nh.advertise<grid_map_msgs::GridMap>(elevation_output_topic_, 1);
  }

private:
  ros::Subscriber sub_;
  ros::Publisher pub_;

  std::string elevation_input_topic_;
  std::string elevation_output_topic_;
  double resolution_;
  double map_x_scale_;
  double map_y_scale_;
  double origin_x_;

  double idw_power_;  // IDW幂参数
  double idw_radius_; // IDW搜索半径

  // 存储有效点的结构体
  struct SamplePoint
  {
    double x, y, value;
  };

  void mapCallback(const grid_map_msgs::GridMapConstPtr &msg)
  {
    grid_map::GridMap input_map;
    grid_map::GridMapRosConverter::fromMessage(*msg, input_map);

    double size_x = map_x_scale_ * resolution_;
    double size_y = map_y_scale_ * resolution_;
    grid_map::Position center(origin_x_, 0.0);

    bool success;
    grid_map::GridMap submap = input_map.getSubmap(center, grid_map::Length(size_x, size_y), success);
    if (!success)
    {
      ROS_WARN("Failed to extract submap. Skipping this map.");
      return;
    }

    // Resize to new resolution
    grid_map::GridMap resampled_map;
    resampled_map.setGeometry(grid_map::Length(size_x, size_y), resolution_, center);
    resampled_map.add("elevation");

    // 收集所有有效样本点
    std::vector<SamplePoint> valid_points;
    for (grid_map::GridMapIterator it(submap); !it.isPastEnd(); ++it)
    {
      grid_map::Position pos;
      submap.getPosition(*it, pos);
      double elevation = submap.at("elevation", *it);
      if (!std::isnan(elevation) && !std::isinf(elevation))
      {
        valid_points.push_back({pos.x(), pos.y(), elevation});
      }
    }

    // 如果没有有效点，直接返回
    if (valid_points.empty())
    {
      ROS_WARN("No valid elevation points found. Skipping interpolation.");
      return;
    }

    // 插值处理
    for (grid_map::GridMapIterator it(resampled_map); !it.isPastEnd(); ++it)
    {
      grid_map::Position pos;
      resampled_map.getPosition(*it, pos);

      // 使用IDW进行插值
      double interpolated_value = interpolateIDW(pos, valid_points);
      resampled_map.at("elevation", *it) = interpolated_value;
    }

    grid_map_msgs::GridMap out_msg;
    grid_map::GridMapRosConverter::toMessage(resampled_map, out_msg);

    grid_map::Time timestamp = input_map.getTimestamp();
    out_msg.info.header.stamp = ros::Time(timestamp / 1000, (timestamp % 1000) * 1e6);

    out_msg.info.header.frame_id = "body";
    pub_.publish(out_msg);
  }

  // IDW插值函数 [[6]]
  double interpolateIDW(const grid_map::Position &target, const std::vector<SamplePoint> &valid_points)
  {
    double sum_weights = 0.0;
    double sum_weighted_values = 0.0;
    bool has_neighbors = false;

    for (const auto &point : valid_points)
    {
      double dx = target.x() - point.x;
      double dy = target.y() - point.y;
      double distance = std::sqrt(dx * dx + dy * dy);

      // 跳过距离为0的点（防止除以0）
      if (distance == 0.0)
      {
        return point.value;
      }

      // 仅使用指定半径内的点
      if (distance <= idw_radius_)
      {
        double weight = 1.0 / std::pow(distance, idw_power_);
        sum_weights += weight;
        sum_weighted_values += weight * point.value;
        has_neighbors = true;
      }
    }

    // 如果没有邻居，返回0
    if (!has_neighbors)
    {
      return 0.0;
    }

    return sum_weighted_values / sum_weights;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "elevation_cliper");
  ros::NodeHandle nh("~");
  ElevationClipper clipper(nh);
  ros::spin();
  return 0;
}